%notes: projected altitude = 229m,

%% Set Constants
g = -9.8; %m/s^2
rho = 1.2041; %kg/m^3
%Rocket Parameters:
rlen = .986; %m
rdia = .046; %m
rmass = .26; %kg
rmoment = rmass*rlen^2/12; %kg*m^2
rareat = pi*rdia^2/4; %m^2
rareaf = rlen*rdia;
nosefin_area = .046^2;
Cd = .05; %unitless (pressure only)
Cl  = .005; %unitless (lift coefficient)

Dg = .5; %m (dist from Cg to nose)
Dp = .3; %m (dist from Cp to Cg)
Dl = -.2; %m (dist from Clift to Cg), magnitude is a guess, but should be opposite of the Cp.
Df = -.4; %m (dist from Cg to nose fins)

D12Curve = [0 0.049 0.116 00.184 00.237 00.282 00.297 00.311 00.322 00.348 00.386 00.442 0.546 0.718 0.879 1.066 1.257 1.436 1.590 1.612 1.650;
            0 2.569 9.369 17.275 24.258 29.730 27.010 22.589 17.990 14.126 12.099 10.808 9.876 9.306 9.105 8.901 8.698 8.310 8.294 4.613 0.000];
%N

%% Prepare Sim
axes = [0 20 0 230];
rel_axes = [-1 1 -2 2];

ts = 0:.01:20;
start_theta = .02; %rad; 0 = up, pi/2 = right, -pi/2 = left
start_pos = [0 0]; %m
start_omega = 0; %rad/s
start_vel = [0 0]; %m/s

%% Run Sim
theta = start_theta;
pos = start_pos;
omega = start_omega;
vel = start_vel;
suntheta = 0;

pt = 0;

Fgravity = [0 g*rmass];
angles = [];
vels = [];
for t = ts
    
    velocity_angle = atan2(vel(1), vel(2));
    sint = sin(theta); cost = cos(theta);
    
    Fthrust = interpolate(D12Curve, t)*[sint, cost];
    Fdrag = -.5*rho*Cd*abs([cost sint]*[vel; vel(2) -vel(1)])*[rareat rareaf]*vel;
    Flift = .5*rho*Cl*rareaf*an_angle*norm(vel)*vel*[0 -1; 1 0];
    
    Tdrag = Dp*dot([-cost sint], Fdrag);
    Tlift = Dl*dot([-cost sint], Flift);
    Tdamping = -.5*rho*Cd*rdia*rlen*norm(rlen/4*omega)*(rlen/4*omega)*rlen/4;
    Tdisturb =  0; %%%
    if rand(1) > .98
        %Tdisturb = (rand(1)) *.4
    end
    
    Ffin = finEffect(vel, theta-suntheta);
    Tfin = Df*dot([-cost sint], Ffin);
    
    Fnet = Fthrust + Fdrag + Flift + Fgravity + Fdragnose + Fliftnose;
    Tnet = Tdrag + Tlift + Tdisturb + Tdamping + Tdragnose + Tliftnose;
    
    dt = t - pt;
    vel = vel + Fnet/rmass*dt;
    pos = pos + vel*dt;
    omega = omega + Tnet/rmoment*dt;
    theta = theta + omega*dt;
    
    %angles = [angles; [theta atan2(vel(1),vel(2))]]; %test
    %vels = [vels; vel];
    
    %plot real-time
    plot([Cg, Cg-rlen]*sin(theta)+pos(1), [Cg, Cg-rlen]*cos(theta)+pos(2));
    hold on
    plot((Cg-rlen)*sin(theta)+pos(1), (Cg-rlen)*cos(theta)+pos(2), 'x');
    plot(Cg*sin(theta)+pos(1), Cg*cos(theta)+pos(2), 'o');
    hold off
    %axis(axes)
    axis([pos(1) pos(1) pos(2) pos(2)] + rel_axes)
    drawnow
    
    pt = t;
end

%% Plot Results






